#include "../includes.h"



double AgingMatrix[4][4],AgingMatrix_backup[4][4];
double Aging_sol[3];

double Holdover_Control;

double AgingStep[MAX_AGING_TABLE_SIZE];
double AgingDAC[MAX_AGING_TABLE_SIZE];


HoldoverCONTROL_	HoldoverCONTROL;
HoldoverCalc_ 		HoldoverCalc;


void HoldoverControl();
void InsertAgingTable(int ControlStep, float ControlDAC, float ControlTemper);

void calc_matrix(void);
void add_matrix(double, double, double); 

void holdover_calculation(void);
void holdover_calculation_new(void); 

void filter_fit(double x[], double y[], int ndata,  double *a, double *b);
void filter_get_linear_fit( double x[], double y[], int s, double* a, double* b); 
  
void Init_HOLDOVER()
{

    int i;

    for( i = 0; i < MAX_AGING_TABLE_SIZE; i++ ) {
     	HoldoverCONTROL.AgingTable[i].DAC	= 0;
     	HoldoverCONTROL.AgingTable[i].TEMPER= 0;
     	HoldoverCONTROL.AgingTable[i].STEP	= 0;
    }

    HoldoverCONTROL.Phase = PHASE_INSERT_AGING_TABLE;

    HoldoverCONTROL.AgingTableIndex = 0;
    HoldoverCONTROL.AgingTableSize	= 0;
    HoldoverCONTROL.NumOfCal		= 0;
    HoldoverCONTROL.NUMofAgingTable	= 0;
/*
    HoldoverCONTROL.NominalAgingDAC	= 0;
    HoldoverCONTROL.NominalAgingTEMPER= 0;
*/

	HoldoverCalc.flag = OFF;
	// 01-03-28
	HoldoverCONTROL.AvgTimeOffset = 0;

	Aging_sol[0] = Aging_sol[1] = Aging_sol[2] = 0;
}



void HoldoverControl()
{
    int i,j;
	double t1;

  	char msg[200];
  	
	double Aging_Intercept, Aging_Slope;


    CONTROL.IntervalCounter = 0;
	recovery_duration = 0;

    CONTROL.HoldoverDuration++;

    if (CONTROL.HoldoverDuration == 1) {

	    InitOffsetTable(&TimeOffsetTable);
    	InitOffsetTable(&TraceOffsetTable);

		RcvAlarm.last_offset = CONTROL.AvgTimeOffset*1E9;
		//RcvAlarm.alarm_start_time = GC.RunningTime;
    }

    // if (CHECKLOCK.HoldoverCounter < STANDBY_KALMAN_INTERVAL)
    // return;

	msg[0] = 0;

  	switch(HoldoverCONTROL.Phase){
 
  		case	PHASE_INIT_AGING_CALCULATION:

    				if( SYSTEM.OPERATION.CTRLMODE == CTRL_LOCKEDtoGPS ){
    					CONTROL.RealDAC = MeanDAC;
    				}
					for( i = 0; i < 4; i++) {
						for( j = 0; j < 4; j++){	AgingMatrix[i][j] = 0;	}
					}

					//Flag set!!!
					AgingMatrix[0][0] =	(double)(HoldoverCONTROL.NumOfCal);

					HoldoverCalc.flag = ON;
					EVENT.CALCULATION = ON;
					HoldoverCalc.step = 0;
					HoldoverCONTROL.Phase = PHASE_CALCULATE_AGING_MATRIX;
					break;

  		case	PHASE_CALCULATE_AGING_MATRIX:

  					if( HoldoverCalc.flag == OFF ) {

						HoldoverCONTROL.Phase = PHASE_CALCULATE_AGING_SOLUTION;

	        			if (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){
    	    				sprintf((char*)MESG,"%.2f %.2f %.2f : %.2f %.2f %.2f : %.2f %.2f %.2f : %.2f %.2f %.2f\r\n",
									AgingMatrix[0][0],AgingMatrix[0][1],AgingMatrix[0][2],
					     			AgingMatrix[1][0],AgingMatrix[1][1],AgingMatrix[1][2],
					     			AgingMatrix[2][0],AgingMatrix[2][1],AgingMatrix[2][2],
					     			AgingMatrix[0][3],AgingMatrix[1][3],AgingMatrix[2][3]);
				        	TransmitMessage((BYTE*)MESG,DEBUG_PORT);

    	    				sprintf((char*)MESG,"%.4f %.4f : %.4f %.4f : %.4f %.4f\r\n",
									HoldoverCalc.normal_x,HoldoverCalc.normal_y,
									HoldoverCalc.plus_x,HoldoverCalc.plus_y,
									HoldoverCalc.minus_x,HoldoverCalc.minus_y);
				        	TransmitMessage((BYTE*)MESG,DEBUG_PORT);
						}
  					}
  					else {
	        			if (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){

    	    				sprintf((char*)MESG,"calculating holdover solution.. flag:%d \r\n",HoldoverCalc.flag);

							TransmitMessage((BYTE*)MESG,PORT_CONTROL);
						}
  					}
	   				break;

  		case	PHASE_CALCULATE_AGING_SOLUTION:
  			
        			if(SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){
        				sprintf((char*)MESG,"Old sol: %.4f %.4f %.4f\r\n",Aging_sol[0],Aging_sol[1],Aging_sol[2]);
			        	TransmitMessage((BYTE*)MESG,PORT_CONTROL);
			        }

        			/*
			        filter_get_linear_fit( 
			        	AgingStep, 
			        	AgingDAC, 
			        	HoldoverCONTROL.AgingTableSize, 
			        	&Aging_Intercept, 
			        	&Aging_Slope
			        );
		         
			        Aging_sol[0] = Aging_Intercept;
			        Aging_sol[1] = Aging_Slope;
			        Aging_sol[2] = 0.0;
			        */

					// Print Table ------------------------------------------------------------------------------------
					//if(SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){
					//	sprintf((char*)MESG,"\r\n========================================\r\n");
			        //	TransmitMessage((BYTE*)MESG,PORT_CONTROL);
					//	for(i=0;i<800;i++){
					//		sprintf((char*)MESG,"AgingStep:%f, AgingDAC:%f\r\n", AgingStep[i], AgingDAC[i]);
			        //		TransmitMessage((BYTE*)MESG,PORT_CONTROL);
			        //	}
			        //}

        			if(SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){
        				sprintf((char*)MESG,"New sol: %.4f %.4f %.4f\r\n",Aging_sol[0],Aging_sol[1],Aging_sol[2]);
			        	TransmitMessage((BYTE*)MESG,PORT_CONTROL);
			        	
			        	//sprintf((char*)MESG,"\r\n========================================\r\n");
			        	//TransmitMessage((BYTE*)MESG,PORT_CONTROL);
			        }

					if(GC.RunningTime<86400*2) {
						//Aging_sol[1] = Aging_sol[1]* ( AGING_FACTOR + (1.0 - AGING_FACTOR)*((double)GC.RunningTime/(double)(86400*2))) ;
					}

					//Aging_sol[1] = Aging_sol[1]*AGING_FACTOR;

    			 	HoldoverCONTROL.Phase = PHASE_AGING_CONTROL;
    			 	HoldoverCONTROL.StepCounter = 0;

    			 	HoldoverCONTROL.InitialTEMPER = TEMPERATURE.AVERAGE;

    			 	HoldoverCONTROL.AgingReference = MeanDAC - (Aging_sol[0]+ Aging_sol[2]*(TEMPERATURE.AVERAGE));

				    
				    /*
					if( ( OSC.kind == Rubidium_OSC ) || HoldoverCalc.plus_y-HoldoverCalc.minus_y > 0.01 || HoldoverCalc.plus_y-HoldoverCalc.minus_y < -0.01 ){

						t1 = ((AgingMatrix[0][0]*AgingMatrix[1][1])-(AgingMatrix[0][1]*AgingMatrix[1][0]));
						Aging_sol[0] = ((AgingMatrix[1][1]*AgingMatrix[0][3])-(AgingMatrix[0][1]*AgingMatrix[1][3]))/t1;
						Aging_sol[1] = ((AgingMatrix[0][0]*AgingMatrix[1][3])-(AgingMatrix[1][0]*AgingMatrix[0][3]))/t1;
						Aging_sol[2] = 0;
					}

					if(GC.RunningTime<86400*2) {
						Aging_sol[1] = Aging_sol[1]* ( AGING_FACTOR + (1.0 - AGING_FACTOR)*((double)GC.RunningTime/(double)(86400*2))) ;
					}

					//Aging_sol[1] = Aging_sol[1]*AGING_FACTOR;

    			 	HoldoverCONTROL.Phase = PHASE_AGING_CONTROL;
    			 	HoldoverCONTROL.StepCounter = 0;

    			 	HoldoverCONTROL.InitialTEMPER = TEMPERATURE.AVERAGE;

    			 	HoldoverCONTROL.AgingReference = MeanDAC - (Aging_sol[0]+ Aging_sol[2]*(TEMPERATURE.AVERAGE));
				    */

        			//if (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){
        			//	sprintf((char*)MESG,"sol: %.4f %.4f %.4f\r\n",Aging_sol[0],Aging_sol[1],Aging_sol[2]);
			        //	TransmitMessage((BYTE*)MESG,DEBUG_PORT);
			        //}

  	         		break;
    }


    //if ( SYSTEM.OPERATION.CTRLMODE == CTRL_LOCKEDtoGPS ){
	    //CONTROL.RealDAC = MeanDAC;
	//}

	if(SYSTEM.OPERATION.LOCKMODE == LOCKED){
	    Modified_RAW_OFFSET = RAW_OFFSET - (SYSTEM.POSITION.CURRENT.HEI-SYSTEM.POSITION.AVERAGE.HEI)/4243.0;
    	InsertTimeOffset(&TimeOffsetTable,(int)(Modified_RAW_OFFSET*100),(int)(RAW_DIFFERENCE*100));
    }


	// Free run
	if( CONTROL.Run_Mode == FREE_RUN ){

		if( CGM_P() ){

		   	if ( SYSTEM.OPERATION.CTRLMODE == CTRL_LOCKEDtoGPS ){
			   	if( HoldoverCONTROL.Phase == PHASE_INSERT_AGING_TABLE ){   //HOLDOVER CONTROL!

			  		if( CONTROL.RealDAC >= (CONFIG.DAC+10) ){

			  			CONTROL.RealDAC -= 10;
			  			MeanDAC = CONTROL.RealDAC;
			  			//SetRbFreq();
			  			//RbSRO_Control();	//sewon 120503

			  		}

			  		else if( CONTROL.RealDAC <= (CONFIG.DAC-10) ){

			  			CONTROL.RealDAC += 10;
			  			MeanDAC = CONTROL.RealDAC;
			  			//SetRbFreq();
			  			//RbSRO_Control();	//sewon 120503
					}

					else {
			  			CONTROL.RealDAC = (double)CONFIG.DAC;
			  			MeanDAC = CONTROL.RealDAC;
			  			//SetRbFreq();
			  			//RbSRO_Control();	//sewon 120503
					}
				}
			}
		}
	}


//  	if( !(GC.ACCsecond%KInterval) ){
  	if( (CONTROL.HoldoverDuration > 10) && (!(GC.ACCsecond%50)) ){

		HoldoverCONTROL.AvgTimeOffset = GetTimeOffset(&TimeOffsetTable,GPS_CUTOFF);

		// Locked Check
		if(SYSTEM.OPERATION.LOCKMODE == LOCKED){
			if( SKIP_RECEIVER_ALARM_CONTROL == ON ){
				SKIP_RECEIVER_ALARM_CONTROL = OFF;
			}
			else {
				CheckReceiverGood(HoldoverCONTROL.AvgTimeOffset);
			}
		}

    	if( HoldoverCONTROL.Phase == PHASE_AGING_CONTROL ){   //HOLDOVER CONTROL!

        	if ( SYSTEM.OPERATION.CTRLMODE == CTRL_LOCKEDtoGPS ){

				if(CONTROL.HoldoverDuration>2*60){
        			Holdover_Control = 	Aging_sol[0]+Aging_sol[1]*(HoldoverCONTROL.StepCounter++)+Aging_sol[2]*(TEMPERATURE.AVERAGE);
        			CONTROL.RealDAC  = 	Holdover_Control + HoldoverCONTROL.AgingReference ;
    			 	 
    			 	//CONTROL.RealDAC = MeanDAC + Aging_sol[1]*(HoldoverCONTROL.StepCounter++)+Aging_sol[2]*(TEMPERATURE.AVERAGE-TEMPERATURE.AVERAGE))

					//SetRbFreq();
		  			//RbSRO_Control();	//sewon 120503

	        		CONTROL.ControlDAC = CONTROL.RealDAC;
	        	}
        	}

        	if (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG){
				sprintf((char*)MESG,"# %.2f %.2f %.2f # Hcont:%.2f ref:%.2f\r\n",
					Aging_sol[0],Aging_sol[1],Aging_sol[2],Holdover_Control,HoldoverCONTROL.AgingReference
				);
        		TransmitMessage((BYTE*)MESG,DEBUG_PORT);
        	}
	    }

       	if(CONTROL.Run_Mode != FREE_RUN){

	        if ((SYSTEM.OPERATION.CTRLMODE == CTRL_LOCKEDtoGPS) && (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG)){

	        	sprintf((char*)MESG,"(SC:%d HSC:%d)%2d:%2d:%2d %3u> R:%7.2f Tmean:%5.2f C:%5.2f TDAC:%5.2f %d\r\n",
	                CONTROL.StepCounter,HoldoverCONTROL.StepCounter, SYSTEM.TIME.CUR.HOUR,SYSTEM.TIME.CUR.MINUTE,SYSTEM.TIME.CUR.SECOND,
	                CONTROL.HoldoverDuration,(float)CONTROL.RealDAC,TEMPERATURE.AVERAGE,TEMPERATURE.current,OCXO_ID.TDAC,GC.ACCsecond);

	        	TransmitMessage((BYTE*)MESG,DEBUG_PORT);
	    	}
	    }
  	}
  	else{
        if ((SYSTEM.OPERATION.CTRLMODE == CTRL_LOCKEDtoGPS) && (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG)){

        	if(CONTROL.Run_Mode == FREE_RUN){

	        	//sprintf((char*)MESG,"<%c>%6d:%2d:%2d:%2d %3u> R:%7.2f M:%7.2f RO:%5.2f OT:%5.2f C:%5.2f TDAC:%5.2f %d,m,%.9le,r_o,%.9le,gCA,%.9le\r\n",
                //	CONTROL.Run_Mode,
                //	CONTROL.StepCounter,SYSTEM.TIME.CUR.HOUR,SYSTEM.TIME.CUR.MINUTE,SYSTEM.TIME.CUR.SECOND,
                //	CONTROL.HoldoverDuration,(float)CONTROL.RealDAC,(float)MeanDAC,RAW_OFFSET,TEMPERATURE.CURRENT,TEMPERATURE.current,OCXO_ID.TDAC,GC.ACCsecond,ModiValue,Recovery_Offset,gCurrentAll);
                //sprintf((char*)MESG,"<%c>%6d:%2d:%2d:%2d %3u> R:%7.2f M:%7.2f RO:%5.2f OT:%5.2f\r\n",
                //	CONTROL.Run_Mode,
                //	CONTROL.StepCounter,SYSTEM.TIME.CUR.HOUR,SYSTEM.TIME.CUR.MINUTE,SYSTEM.TIME.CUR.SECOND,
                //	CONTROL.HoldoverDuration,(float)CONTROL.RealDAC,(float)MeanDAC,RAW_OFFSET,TEMPERATURE.CURRENT
                //);
                sprintf((char*)MESG,"<%c>%6d:%2d:%2d:%2d %3u> R:%7.2f M:%7.2f RO:%5.2f T:%5.2f TD:%5.2f\r\n",
                	CONTROL.Run_Mode,
                	CONTROL.StepCounter,SYSTEM.TIME.CUR.HOUR,SYSTEM.TIME.CUR.MINUTE,SYSTEM.TIME.CUR.SECOND,
                	CONTROL.HoldoverDuration,(float)CONTROL.RealDAC,(float)MeanDAC,
                	(float)RAW_OFFSET,(float)TEMPERATURE.CURRENT,(float)OCXO_ID.TDAC
                );
        	}
        	else {
	        	//sprintf((char*)MESG,"<%d>#%d#%d#%d##%6d:%2d:%2d:%2d [H]%3u> R:%7.2f M:%7.2f RO:%5.2f OT:%5.2f C:%5.2f TDAC:%5.2f %d,m,%.9le,r_o,%.9le,gCA,%.9le\r\n",
                //	HoldoverCONTROL.Phase,
                //	HoldoverCONTROL.NUMofAgingTable,
                //	HoldoverCONTROL.NumOfCal,
                //	HoldoverCONTROL.AgingTableSize,
                //
                //	CONTROL.StepCounter,SYSTEM.TIME.CUR.HOUR,SYSTEM.TIME.CUR.MINUTE,SYSTEM.TIME.CUR.SECOND,
                //	CONTROL.HoldoverDuration,(float)CONTROL.RealDAC,(float)MeanDAC,RAW_OFFSET,TEMPERATURE.CURRENT,TEMPERATURE.current,OCXO_ID.TDAC,GC.ACCsecond,ModiValue,Recovery_Offset,gCurrentAll);
                sprintf((char*)MESG,"<%d>#%d#%d#%d##%6d:%2d:%2d:%2d [H]%3u> R:%7.2f M:%7.2f RO:%5.2f T:%5.2f TD:%5.2f\r\n",
                	HoldoverCONTROL.Phase,
                	HoldoverCONTROL.NUMofAgingTable,
                	HoldoverCONTROL.NumOfCal,
                	HoldoverCONTROL.AgingTableSize,
                
                	CONTROL.StepCounter,SYSTEM.TIME.CUR.HOUR,SYSTEM.TIME.CUR.MINUTE,SYSTEM.TIME.CUR.SECOND,
                	CONTROL.HoldoverDuration,(float)CONTROL.RealDAC,(float)MeanDAC,
                	(float)RAW_OFFSET,(float)TEMPERATURE.CURRENT,(float)OCXO_ID.TDAC
                );
			}
       		TransmitMessage((BYTE*)MESG,DEBUG_PORT);
      	}
  	}



}


void InsertAgingTable(int ControlStep, float ControlDAC, float ControlTemper)
{

    if(ControlStep > START_POINT_OF_MAKING_AGING_TABLE || debug_calc){   // HOLDOVER CONTROL

		if(ControlStep&0x01){

            HoldoverCONTROL.AgingTableIndex = HoldoverCONTROL.NUMofAgingTable % MAX_AGING_TABLE_SIZE;

            HoldoverCONTROL.AgingTable[HoldoverCONTROL.AgingTableIndex].DAC 	= ControlDAC;
            HoldoverCONTROL.AgingTable[HoldoverCONTROL.AgingTableIndex].TEMPER 	= ControlTemper;
            HoldoverCONTROL.AgingTable[HoldoverCONTROL.AgingTableIndex].STEP 	= GC.RunningTime/50;
             
            //----------------------------------------------------------------------------------------------------------------
            HoldoverCONTROL.AgingTable_new[HoldoverCONTROL.AgingTableIndex].DAC 	= ControlDAC;
            HoldoverCONTROL.AgingTable_new[HoldoverCONTROL.AgingTableIndex].TEMPER 	= 0.0;
            HoldoverCONTROL.AgingTable_new[HoldoverCONTROL.AgingTableIndex].STEP 	= GC.RunningTime/50;
             
            AgingStep[HoldoverCONTROL.AgingTableIndex] = HoldoverCONTROL.AgingTable_new[HoldoverCONTROL.AgingTableIndex].STEP;
			AgingDAC[HoldoverCONTROL.AgingTableIndex]= HoldoverCONTROL.AgingTable_new[HoldoverCONTROL.AgingTableIndex].DAC; 
            //----------------------------------------------------------------------------------------------------------------

            HoldoverCONTROL.NUMofAgingTable++;

            if(HoldoverCONTROL.NUMofAgingTable <= MAX_AGING_TABLE_SIZE){

             	HoldoverCONTROL.AgingTableSize = HoldoverCONTROL.NumOfCal = HoldoverCONTROL.NUMofAgingTable;
            }
            else {
            	HoldoverCONTROL.AgingTableSize = HoldoverCONTROL.NumOfCal = MAX_AGING_TABLE_SIZE;
            }


            if ((SYSTEM.OPERATION.CTRLMODE == CTRL_LOCKEDtoGPS) && (SYSTEM.IO.CTRL_PORT.MODE == UDEBUG)){

		       	sprintf((char*)MESG,"<%d>#%d#%d#%d# %.2f %.2f %d\r\n",
           	        HoldoverCONTROL.Phase,
           	        HoldoverCONTROL.NUMofAgingTable,
           	        HoldoverCONTROL.AgingTableIndex,
           	        HoldoverCONTROL.AgingTableSize,
                    HoldoverCONTROL.AgingTable[HoldoverCONTROL.AgingTableIndex].DAC,
                    HoldoverCONTROL.AgingTable[HoldoverCONTROL.AgingTableIndex].TEMPER,
                    HoldoverCONTROL.AgingTable[HoldoverCONTROL.AgingTableIndex].STEP);

           		TransmitMessage((BYTE*)MESG,DEBUG_PORT);
			}

			if(HoldoverCONTROL.AgingTableSize > MINIMUM_TABLE_BOUND_OF_AGING_CONTROL){

            	HoldoverCONTROL.Phase = PHASE_INIT_AGING_CALCULATION;

            	if( SYSTEM.EB.STATUS == EB_STATUS_RUN){
            		CONTROL.Run_Mode = REFERENCE_GPS_RUN;
            	}
            	else if(SYSTEM.EB.STATUS == EB_STATUS_8K_RUN){
            		CONTROL.Run_Mode = REFERENCE_8K_RUN;
            	}
            }
		}
    }
}


void add_matrix(double dac, double step, double temper)
{
//		AgingMatrix[0][0] =	(double)(table_size);
		AgingMatrix[0][1] += step;
		AgingMatrix[0][2] += temper;

		AgingMatrix[1][0] =	AgingMatrix[0][1];
		AgingMatrix[1][1] += step*step;
		AgingMatrix[1][2] += step*temper;

		AgingMatrix[2][0] =	AgingMatrix[0][2];
		AgingMatrix[2][1] =	AgingMatrix[1][2];
		AgingMatrix[2][2] += temper*temper;

		AgingMatrix[0][3] += dac;
		AgingMatrix[1][3] += step*dac;
		AgingMatrix[2][3] += temper*dac;

}


/*********************************************************************************************/
//	Global :AgingMatrix
//			AgingMatrix_plus
//			AgingMatrix_minus
//			HoldoverCONTROL.NumOfCal
//
//	Aging Table
/*********************************************************************************************/
void holdover_calculation()
{
	int i,j;

	if( HoldoverCONTROL.NumOfCal > 1 ){
		add_matrix((double)HoldoverCONTROL.AgingTable[HoldoverCONTROL.NumOfCal-1].DAC,(double)(HoldoverCONTROL.AgingTable[HoldoverCONTROL.NumOfCal-1].STEP),(double)HoldoverCONTROL.AgingTable[HoldoverCONTROL.NumOfCal-1].TEMPER);
 		HoldoverCONTROL.NumOfCal--;
 	}
	else if(HoldoverCONTROL.NumOfCal == 1 ){

		switch( HoldoverCalc.step ){
			// plus
			case 0:
					for( i = 0; i < 4; i++ ){
						for( j = 0; j < 4; j++ ){	AgingMatrix_backup[i][j] = AgingMatrix[i][j];	}
											}
					add_matrix((double)HoldoverCONTROL.AgingTable[0].DAC,(double)(HoldoverCONTROL.AgingTable[0].STEP),(double)HoldoverCONTROL.AgingTable[0].TEMPER+1);
					HoldoverCalc.step++;
					break;
			case 1:
					calc_matrix();
					HoldoverCalc.plus_x = Aging_sol[1];		HoldoverCalc.plus_y = Aging_sol[2];
					HoldoverCalc.step++;
					break;
			// minus
			case 2:
					for( i = 0; i < 4; i++ ){
						for( j = 0; j < 4; j++ ){	AgingMatrix[i][j] = AgingMatrix_backup[i][j];	}
											}
					add_matrix((double)HoldoverCONTROL.AgingTable[0].DAC,(double)(HoldoverCONTROL.AgingTable[0].STEP),(double)HoldoverCONTROL.AgingTable[0].TEMPER-1);
					HoldoverCalc.step++;
					break;
			case 3:
					calc_matrix();
					HoldoverCalc.minus_x = Aging_sol[1];		HoldoverCalc.minus_y = Aging_sol[2];
					HoldoverCalc.step++;
					break;
			// normal
			case 4:
					for( i = 0; i < 4; i++ ){
						for( j = 0; j < 4; j++ ){	AgingMatrix[i][j] = AgingMatrix_backup[i][j];	}
					}
					add_matrix((double)HoldoverCONTROL.AgingTable[0].DAC,(double)(HoldoverCONTROL.AgingTable[0].STEP),(double)HoldoverCONTROL.AgingTable[0].TEMPER);
					HoldoverCalc.step++;
					break;
			case 5:
					calc_matrix();
					HoldoverCalc.normal_x = Aging_sol[1];	HoldoverCalc.normal_y = Aging_sol[2];
					HoldoverCalc.flag = OFF;
					break;
		}
	}
}


void calc_matrix(void)
{

	double p1,p2,p3;

	double t1matrix[3][3];
	double t2matrix[2][2];
	double t3matrix[3][3];

	double solmat[2][2];
	
	p1 = AgingMatrix[1][0]/AgingMatrix[0][0];	// average time: AgingMatrix[0][1] += step; 
	
	t1matrix[0][0] =	AgingMatrix[0][1]*p1;   // sum of time * average time
	t1matrix[0][1] =	AgingMatrix[0][2]*p1;
	t1matrix[0][2] =	AgingMatrix[0][3]*p1;

	t3matrix[0][0]	=	AgingMatrix[1][1]-t1matrix[0][0]; //AgingMatrix[1][1] += step*step;  - average time
	t3matrix[0][1]	=	AgingMatrix[1][2]-t1matrix[0][1];
	t3matrix[0][2]	=	AgingMatrix[1][3]-t1matrix[0][2]; 

	p2 = AgingMatrix[2][0]/AgingMatrix[0][0];

	t1matrix[1][0] =	AgingMatrix[0][1]*p2;
	t1matrix[1][1] =	AgingMatrix[0][2]*p2;
	t1matrix[1][2] =	AgingMatrix[0][3]*p2;

	t3matrix[1][0]	=	AgingMatrix[2][1]-t1matrix[1][0];
	t3matrix[1][1]	=	AgingMatrix[2][2]-t1matrix[1][1];
	t3matrix[1][2]	=	AgingMatrix[2][3]-t1matrix[1][2];

		//AgingMatrix[0][1] += step;
		//AgingMatrix[0][2] += temper;
        //
		//AgingMatrix[1][0] =	AgingMatrix[0][1];
		//AgingMatrix[1][1] += step*step;
		//AgingMatrix[1][2] += step*temper;
        //
		//AgingMatrix[2][0] =	AgingMatrix[0][2];
		//AgingMatrix[2][1] =	AgingMatrix[1][2];
		//AgingMatrix[2][2] += temper*temper;
        //
		//AgingMatrix[0][3] += dac;
		//AgingMatrix[1][3] += step*dac;
		//AgingMatrix[2][3] += temper*dac; 

	/*  
		
		#### ####
		####//t3matrix[1][2]:16.841858  
		//t3matrix[1][2]	=	AgingMatrix[2][3]-t1matrix[1][2]; << sum(temper*dac ) - (sum of dac * average temper )
		** AgingMatrix[2][3]
		//AgingMatrix[2][3] += temper*dac;  
		
		**t1matrix[1][2]
		t1matrix[1][2] = AgingMatrix[0][3]*p2;  << sum of dac * average temper
		p2 = AgingMatrix[2][0]/AgingMatrix[0][0]; <-- average temper
		AgingMatrix[0][3] += dac;
		AgingMatrix[2][0] =	AgingMatrix[0][2]  += temper;
		
		#### ####
		####//t2matrix[0][1]:16.841858 
		t2matrix[0][1] =	t3matrix[0][2]*p3;  << sum( step*dac; ) - ( sum of dac * average time )  *  { [ sum(step*temper) - sum of time *average temper  ] / 800  }
		**
		t3matrix[0][2]	=	AgingMatrix[1][3]-t1matrix[0][2]; << sum( step*dac; ) - ( sum of dac * average time )
		t1matrix[0][2] =	AgingMatrix[0][3]*p1; << sum of dac * average time
				p1 = AgingMatrix[1][0]/AgingMatrix[0][0];	// average time: AgingMatrix[0][1] += step; 
				AgingMatrix[1][0] =	AgingMatrix[0][1] += step;
		
		**
		p3
		p3 = t3matrix[1][0]/t3matrix[0][0];  << [ sum(step*temper) - sum of time *average temper  ] / 800
			**
			t3matrix[1][0]	=	AgingMatrix[2][1]-t1matrix[1][0]; << sum(step*temper) - sum of time *average temper
								                **t1matrix[1][0] = AgingMatrix[0][1]*p2;
			
		
		Temper:0.093750
		case 5-----
		t3matrix[1][0]:62.343750, t3matrix[1][1]:5.844727, 
		t2matrix[0][0]:5.844727, t2matrix[0][1]:16.841858
		
	*/
	
	p3 = t3matrix[1][0]/t3matrix[0][0];

	t2matrix[0][0] =	t3matrix[0][1]*p3;
	t2matrix[0][1] =	t3matrix[0][2]*p3;

	solmat[0][0] =	t3matrix[1][1]-t2matrix[0][0];
	solmat[0][1] =	t3matrix[1][2]-t2matrix[0][1];
	Aging_sol[2] =	solmat[0][1]/solmat[0][0];
	Aging_sol[1] =	(t3matrix[0][2]-t3matrix[0][1]*Aging_sol[2])/t3matrix[0][0];
	Aging_sol[0] =	(AgingMatrix[0][3]-AgingMatrix[0][1]*Aging_sol[1]-AgingMatrix[0][2]*Aging_sol[2])/AgingMatrix[0][0];
	

//	Aging_sol[1] = Aging_sol[1]*AGING_FACTOR;
//	fprintf(stdout,"x:%.4f y:%.4f t1:%.4f\n",x,y,t1);
}



void filter_fit(double x[], double y[], int ndata,  double *a, double *b)
{
	int i;
	double t,sxoss,sx=0.0,sy=0.0,st2=0.0,ss;

    double sigav, sigbv, chi2v, qv;

    double *siga, *sigb, *chi2, *q;

    siga=&sigav; sigb = &sigbv; chi2=&chi2v; q = &qv;

	*b=0.0;

	for (i=1;i<=ndata;i++) {
		sx += x[i];
		sy += y[i];
	}
	ss=ndata;

	sxoss=sx/ss;

	for (i=1;i<=ndata;i++) {
		t=x[i]-sxoss;
		st2 += t*t;
		*b += t*y[i];
	}

	*b = *b/st2;
	*a=(sy-sx*(*b))/ss;
	*siga=sqrt((1.0+sx*sx/(ss*st2))/ss);
	*sigb=sqrt(1.0/st2);

	*chi2=0.0;
	*q=1.0;

}

void filter_get_linear_fit( double x[], double y[], int s, double* a, double* b)
{
    s-=1;
	filter_fit(x, y, s, a, b );
}




